#include "canMotor.hpp"

#define LOG_TAG "driver_motor"
#define LOG_LVL LOG_LVL_DBG
#include <ulog.h>

robot_motor* robot_motor::p = RT_NULL;

robot_motor& robot_motor::instance()
{
    if(p == RT_NULL)
    {
        p = new robot_motor(robot_can_bus::instance());
        p->open();
    }
    return *p;
}

robot_motor::robot_motor(robot_can_bus* b):robot_can_dev(b)
{
    /* for receive boot up */
    add_id(0x700);
    add_id(0x701);

    return;

    for(int i=0; i<max_motor_count; i++)
    {
        bp_flag[i] = NBootup;
        op_flag[i] = Preope;
        move_mode_flag[i] = Mode_unset;
    }
}

robot_motor::~robot_motor()
{
    return;
}

void robot_motor::wait_boot_up(Motor_ID mid)
{
    rt_can_msg r_msg;
    rt_can_msg s_msg;
    
    read_clear();

    rt_uint32_t can_id = 0x700 + mid;

    while(bp_flag[mid] == NBootup) // wait motor send boot up msg
    {
        if(read(0, &r_msg, sizeof(r_msg)) == 0)
        {
            rt_thread_delay(1000);

            s_msg.id = can_id;
            s_msg.rtr = RT_CAN_RTR;
            write(0, &s_msg, sizeof(s_msg));
        }
        else
        {
            if(r_msg.id == can_id)
                bp_flag[mid] = Bootup;
        }
        
    }
}

void robot_motor::cmode_from_unset(Motor_ID mid)
{
    rt_can_msg r_msg;
    rt_can_msg s_msg;

    rt_uint32_t can_id = 0x700 + ID(mid);
    wait_boot_up(mid);
    read_clear();

    while(op_flag[ID(mid)] == Preope)
    {
        s_msg.id = can_id;
        s_msg.rtr = RT_CAN_RTR;
        write(0, &s_msg, sizeof(s_msg));
        rt_thread_delay(10);
        read_block(0, &r_msg, sizeof(r_msg));
        if(r_msg.id==can_id && (r_msg.data[0]&0x0F)==0x05)//operation
        {
            op_flag[ID(mid)] = Ope;
        }
        else if(r_msg.id==can_id && (r_msg.data[0]&0x7F)==0x7F)//pre-operation
        {
            s_msg.id = can_id;
            s_msg.rtr = RT_CAN_DTR;
            s_msg.len = 1;
            s_msg.data[0] = 0x01;
            write(0, &s_msg, sizeof(s_msg));
        }
    }
}

void robot_motor::cmode_to_init(Motor_ID mid)
{
    rt_can_msg r_msg;
    rt_can_msg s_msg;

    rt_uint32_t can_id = 0x200 + ID(mid);

    if(move_mode_flag[ID(mid)] = Mode_unset)
    {
        cmode_from_unset(mid);
    }

    s_msg.id = can_id;
    s_msg.rtr = RT_CAN_DTR;
    s_msg.len = 2;
    s_msg.data[0] = 0x06;
    s_msg.data[1] = 0x00;
    write(0, &s_msg, sizeof(s_msg));

    s_msg.data[0] = 0x07;
    write(0, &s_msg, sizeof(s_msg));


    s_msg.data[0] = 0x0F;
    write(0, &s_msg, sizeof(s_msg));

    move_mode_flag[ID(mid)] = Mode_init;
}


/**
 * @brief function for user
 * @param  mid    motor id
 */
void robot_motor::motor_init(Motor_ID mid)
{
    cmode_to_init(mid);
}


/**
 * @brief 
 * @param  mid              My Param doc
 */
void robot_motor::motor_set_modespeed(Motor_ID mid)
{
    rt_can_msg s_msg;
    rt_uint32_t can_id = 0x600 + ID(mid);

    /* check if init */
    if(move_mode_flag[mid] == Mode_unset)
    {
        cmode_to_init(mid);
    }

    /* check mode */
    if(move_mode_flag[mid] != Mode_speed)
    {
        s_msg.id = can_id;
        s_msg.rtr = RT_CAN_DTR;
        s_msg.len = 5;
        s_msg.data[0] = 0x2F;
        s_msg.data[1] = 0x60;
        s_msg.data[2] = 0x60;
        s_msg.data[3] = 0x00;
        s_msg.data[4] = 0x03;
        write(0, &s_msg, sizeof(s_msg));
    }
}


/**
 * @brief 
 * @param  mid              My Param doc
 */
void robot_motor::motor_set_modedist(Motor_ID mid)
{
    rt_uint32_t can_id = 0x600 + ID(mid);
    rt_can_msg s_msg;
    /* check if init */
    if(move_mode_flag[mid] == Mode_unset)
    {
        cmode_to_init(mid);
    }

    /* check mode */
    if(move_mode_flag[mid] != Mode_dist)
    {
        s_msg.id = can_id;
        s_msg.rtr = RT_CAN_DTR;
        s_msg.len = 5;
        s_msg.data[0] = 0x2F;
        s_msg.data[1] = 0x60;
        s_msg.data[2] = 0x60;
        s_msg.data[3] = 0x00;
        s_msg.data[4] = 0x01;
        write(0, &s_msg, sizeof(s_msg));
    }
}


/**
 * @brief stop motor
 * @param  mid   motor id
 */
void robot_motor::motor_stop(Motor_ID mid)
{
    rt_can_msg r_msg;
    rt_can_msg s_msg;

    rt_uint32_t can_id = 0x410 + ID(mid);

    /* check mode */
    if(move_mode_flag[mid] == Mode_speed)
    {
        s_msg.id = can_id;
        s_msg.rtr = RT_CAN_DTR;
        s_msg.len = 6;
        s_msg.data[0] = 0x0F;
        s_msg.data[1] = 0x00;
        s_msg.data[2] = 0x00;
        s_msg.data[3] = 0x00;
        s_msg.data[4] = 0x00;
        s_msg.data[5] = 0x00;
        write(0, &s_msg, sizeof(s_msg));
    }

    else if(move_mode_flag[mid] == Mode_dist)
    {

        can_id = 0x210 + ID(mid);

        s_msg.id = can_id;
        s_msg.rtr = RT_CAN_DTR;
        s_msg.len = 2;
        s_msg.data[0] = 0x07;
        s_msg.data[1] = 0x00;
        write(0, &s_msg, sizeof(s_msg));

        s_msg.data[0] = 0x0F;
        write(0, &s_msg, sizeof(s_msg));
    }  
}

/**
 * @brief function for user
 * @param  mid    motor id 
 * @param  data   MSB
 * @param  len    
 */
void robot_motor::motor_set_speed(Motor_ID mid, rt_uint8_t data[], int len)
{
    rt_can_msg r_msg;
    rt_can_msg s_msg;
    rt_uint32_t can_id;

    motor_set_modespeed(mid);

    /* send speed by pdo */
    can_id = 0x400 + ID(mid);

    s_msg.id = can_id;
    s_msg.len = 6;
    s_msg.data[0] = 0x0F;
    s_msg.data[1] = 0x00;
    for(int i=0; i<4; i++)
    {
        s_msg.data[i+2] = data[3-i];
    }
    write(0, &s_msg, sizeof(s_msg));
}


/**
 * @brief set move distance
 * @param  mid             
 * @param  data           
 * @param  len             
 */
void robot_motor::motor_set_dist(Motor_ID mid, rt_uint8_t data[], int len)
{
    rt_can_msg r_msg;
    rt_can_msg s_msg;
    rt_uint32_t can_id;

    motor_set_modedist(mid);

    /* send distance by pdo */
    can_id = 0x310 + ID(mid);
    s_msg.id = can_id;
    s_msg.len = 6;
    s_msg.data[0] = 0x0F;
    s_msg.data[1] = 0x00;
    for(int i=0; i<4; i++)
    {
        s_msg.data[i+2] = data[3-i];
    }
    write(0, &s_msg, sizeof(s_msg));

    can_id = 0x210 + ID(mid);
    s_msg.id = can_id;
    s_msg.len = 2;
    s_msg.data[0] = 0x7F;
    s_msg.data[1] = 0x00;
    write(0, &s_msg, sizeof(s_msg));

    s_msg.data[0] = 0x7F;
    write(0, &s_msg, sizeof(s_msg));
}


/**
 * @brief enable
 * @param  mid            
 */
void robot_motor::motor_enable(Motor_ID mid)
{
    rt_uint32_t can_id = 0x200 + ID(mid);
    rt_can_msg s_msg;

    s_msg.id = can_id;
    s_msg.len = 2;
    s_msg.data[0] = 0x06;
    s_msg.data[1] = 0x00;
    write(0, &s_msg, sizeof(s_msg));

    s_msg.data[0] = 0x07;
    write(0, &s_msg, sizeof(s_msg));

    s_msg.data[0] = 0x0F;
    write(0, &s_msg, sizeof(s_msg));
}

/**
 * @brief disable
 * @param  mid            
 */
void robot_motor::motor_disable(Motor_ID mid)
{
    rt_uint32_t can_id = 0x200 + ID(mid);
    rt_can_msg s_msg;

    s_msg.id = can_id;
    s_msg.len = 2;
    s_msg.data[0] = 0x05;
    s_msg.data[1] = 0x00;
    write(0, &s_msg, sizeof(s_msg));
}


/**
 * @brief reset
 * @param  mid             
 */
void robot_motor::motor_reset(Motor_ID mid)
{
    rt_uint32_t can_id = 0x200 + ID(mid);
    rt_can_msg s_msg;

    s_msg.id = can_id;
    s_msg.len = 2;
    s_msg.data[0] = 0x08;
    s_msg.data[1] = 0x00;
    write(0, &s_msg, sizeof(s_msg));

    s_msg.data[0] = 0x06;
    write(0, &s_msg, sizeof(s_msg));

    s_msg.data[0] = 0x07;
    write(0, &s_msg, sizeof(s_msg));

    s_msg.data[0] = 0x0F;
    write(0, &s_msg, sizeof(s_msg));
}

/**
 * @brief qstop
 * @param  mid             
 */
void robot_motor::motor_qstop(Motor_ID mid)
{
    rt_uint32_t can_id = 0x200 + ID(mid);
    rt_can_msg s_msg;

    s_msg.id = can_id;
    s_msg.len = 2;
    s_msg.data[0] = 0x05;
    s_msg.data[1] = 0x00;
    write(0, &s_msg, sizeof(s_msg));

    /* delay to reset */
    rt_thread_delay(1000);

    s_msg.data[0] = 0x06;
    write(0, &s_msg, sizeof(s_msg));

    s_msg.data[0] = 0x07;
    write(0, &s_msg, sizeof(s_msg));

    s_msg.data[0] = 0x0F;
    write(0, &s_msg, sizeof(s_msg));
}



